template<class Resample>
class PGibbs
{
	public:

PGibbs(imat X, Resample *R)
{
	_X=X;
	_p=X.n_cols;
	_n=X.n_rows;
	mat B(_p,_p);
	B.fill(1);
	_B=diagmat(B);
	_alpha=3;
	_beta=1;
	boost::random::normal_distribution<> N2;
	rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
	boost::random::uniform_01<> U;

	_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
	_d=new boost::math::normal_distribution<>(0,1); 
	_R=R;
}
double sigma_update(mat m, mat Z)
{
	double s=0;
	double a=_alpha+_n*_p/2;
	double b=_beta;
	for(int j=0;j<_p;j++)
	{
		for(int i=0;i<_n;i++)
		{
			b+=0.5*(Z(i,j)-m(0,j))*(Z(i,j)-m(0,j));
			//cout << "b " <<  b << "Z " << Z(i,j) << "m " << m(0,j); 
			
		}
		//cout << "\n";
	}
		cout << "a " << a << "b " << (double)1/b;
		boost::random::gamma_distribution<> Ni(a,(double)1/b);
		RandomG::Random<boost::random::gamma_distribution<> > rgamma(Ni);
		s=(double)1/rgamma();
	//cout << s;
	return s;
}
mat m_update(double sigma, mat Z)
{
	mat zb=sum(Z,0);	
	
	mat S(_p,1);
	mat me(_p,1);
	mat m(_p,1);
	for(int i=0;i<_p;i++)
	{
		S(i,0)=sigma/((_n+sigma/_B(i,i)));
		me(i,0)=zb(0,i)/(_n+sigma/_B(i,i));
		m(i,0)=(*rnorm)()*sqrt(S(i,0))+me(i,0);	
	}
	//cout << "m " << m.t();
	return m.t();
}
mat cond_PF( mat m, double s, imat x, int Mx, mat cz)
{
	mat z(Mx,_p);
	mat W(Mx,_p);
	z.fill(0);
	W.fill(0);
	mat ks(_p,1);
	//cout << "x" << x;
	z(0,span::all)=cz;
	int k1=Searchindex(x,1);
	ks(0,0)=k1;
	double *w=new double[Mx];
	double cd,arg,u;
	//cout << "k1 " << k1 << "\n"; 
	for(int i=0;i<Mx;i++)
	{
		if(i!=0) z(i,k1)=(*rnorm)()*sqrt(s)+m(0,k1);
		w[i]=1;
		W(i,k1)=1;
	}
	w[0]=1;

	int k_1=k1;
	for(int t=1;t<_p;t++)
	{
		//Resample
		//	cout << "ess "<< ess<< " " ;
		
		if(t!=1)
		{
			mat v=growingvect(Mx);
			int M=Mx-1;
			mat v2=(*_R)(&M,w,v);
			mat vect(1,1);
			vect(0,0)=0;
			mat v3=rbind<mat>(vect,v2);
			mat Z=z(span::all,k_1);
			Arangemat(Z,v3);
			z(span::all,k_1)=Z;
			for(int i=0;i<Mx;i++)
			{
				w[i]=1;
			}
		}
		//Move
		k1=Searchindex(x,t+1);
		ks(t,0)=k1;
		//cout << "i " << t+1<< "k1 " << k1 << "\n"; 
		double tm=m(0,k1);
		double s2=sqrt(s);
		for(int i=0;i<Mx;i++)
		{
		//	cout << "sk1 " << s(0,k1);

			cd=cdf(*_d,(z(i,k_1)-tm)/s2);
			if(i!=0)
			{
				u = (*_runif)(); 
				arg=u*cd;
				if(arg > .999999999) arg=.999999999;
				if(arg < .0000000001) arg=.0000000001;
				z(i,k1)=tm+quantile(*_d,arg)*s2;
				//cout <<" " <<z(i,k1);	
			}
			w[i]=cd;
			W(i,k1)=cd;
			
		}
		k_1=k1;

	}
	//pick N*:
	//cout <<"W "<< W;
	int l=1;
	mat resf(_p,1);	
	resf.fill(0);
	mat v=growingvect(Mx);
	mat tmp=(*_R)(&l,w,v);
	int bt=(int)tmp(0,0);
	resf(k1,0)=z(bt,k1);
	//cout << "k1 " << k1;
	k_1=k1;
	int b_t=bt;
	for(int t=_p-1;t>0;t--)
	{
	//	cout << "bt " << bt;	
	
	//	cout << "w";
	//	if(t!=1)
	//	{
			k1=ks(t-1,0);//Searchindex(x,t);
			
			double tm=m(0,k_1);
			double s2=sqrt(s);
			
			for(int i=0;i<Mx;i++)
			{
				if(z(b_t,k_1)<z(i,k1))
				{
					w[i]=W(i,k1)*pdf(*_d,(z(b_t,k_1)-tm)/s2)/(s2/*cdf(*_d,(z(i,k1)-tm)/s2)*/);
				}else{
					w[i]=0;
				}
				//	cout << "w: " << w[i] << " ";
				//cout << "cdf " << W(i,k1)*pdf(*_d,(z(b_t,k_1)-tm)/s2)/(s2*cdf(*_d,(z(i,k1)-tm)/s2));	
			}
	/*	}else{
			k1=Searchindex(x,t);
			double tm=m(0,k_1);
			double s2=sqrt(s);
			for(int i=0;i<Mx;i++)
			{
				w[i]=pdf(*_d,(z(b_t,k_1)-tm)/s2)/s2;
				//	cout << w[i] << " ";	
			}
		}*/	
	//	cout << "\n";		
		v=growingvect(Mx);
		tmp=(*_R)(&l,w,v);
		bt=(int)(tmp(0,0));
		resf(k1,0)=z(bt,k1);
		//cout << "k1 " << k1;
		k_1=k1;
		b_t=bt;
	}
	
		//cout << "bt " << bt;	
		//cout <<"z " <<  z;
		//cout << "res " << resf;
	delete[] w;
	return resf;

}

vector<mat> operator()(int M, int Mx){

	mat m(M,_p);
	m.fill(0);
	mat sig(M,1);
	sig.fill(1);
	mat z=InitZ();
	cout << z;
	for(int i=1;i<M;i++)
	{
			
			mat tmp;
			for(int j=0;j<_n;j++)
			{
				tmp=cond_PF(m(i-1,span::all), sig(i-1,0),_X(j,span::all),Mx, z(j,span::all));
				z(j,span::all)=tmp.t();
			}
	//	cout <<"z "<< z << " zend";
		m(i,span::all)=m_update(sig(i-1,0),z);
		sig(i,0)=sigma_update(m(i,span::all),z);
		cout <<"m "  <<  m(i,span::all) << ".";
	}	
	vector<mat> vec;
	vec.push_back(m);
	vec.push_back(sig);
	return vec;	
}


mat InitZ(void)
{
	mat z(_n,_p);
	for(int i=0;i<_n;i++)
	{
		for(int j=0;j<_p;j++)
		{
			z(i,j)=(-(double)_X(i,j)+_p/2)/_p;
		}

	}
	return z;
}

	private:
	RandomG::Random<boost::random::normal_distribution<> > *rnorm;
	Resample *_R;
	boost::math::normal_distribution<> *_d;
	int _p;
	int _n;
	mat _B;
	imat _X;
	int _alpha;
	int _beta;
	RandomG::Random<boost::random::uniform_01<> > *_runif;
};
